module matrixfuns
implicit none
contains
!function diag(v)
!double precision v(:)
!double precision diag(1:size(v),1:size(v))
!integer i,j
!forall(i=1:size(v),j=1:size(v))
!	diag(i,j)=0.0d0
!end forall
!forall(i=1:size(v))
!	diag(i,i)=v(i)	
!end forall
!end function diag
function matrizer(rho,n)
integer n,i,j,inic,fin,step
double precision rho(n*(n-1)/2),matrizer(n,n)

inic=1
step=n-1
fin=n-1
do i=1,n-1,1
	matrizer(i+1:n,i)=rho(inic:fin)
	step=step-1
	inic=fin+1
	fin=fin+step
end do
forall(i=1:n)
	matrizer(i,i)=1.0d0
end forall
do i=1,n-1,1
	do j=2,n,1
		matrizer(i,j)=matrizer(j,i)
	end do
end do
end function matrizer

function vec2mat(rho,n)
integer n,i
double precision rho(n*n),vec2mat(n,n)

forall(i=1:n)
	vec2mat(:,i)=rho(n*(i-1)+1:n*i)
end forall
end function vec2mat


function matent(n)
!En-transposed
integer n,i,j
double precision matent(n,n*n)
forall(i=1:n,j=1:n*n)
	matent(i,j)=0.0d0
end forall
forall(i=1:n)
	matent(i,n*(i-1)+i)=1.0d0
end forall
end function matent
function commatrix(m,n)
!Commutation matrix
use linear_operators
integer m,n,i,j
double precision me(1:m,1:m),commatrix(m*n,m*n)&
			   &,ei(1:m,1:1),en(1:n,1:n)

me=eye(m)
en=eye(n)
forall(i=1:m*n,j=1:m*n)
commatrix(i,j)=0.0d0
end forall
do i=1,m,1
    ei(:,1)=me(:,i)
    commatrix=commatrix+kron(kron(ei,en),.t.ei)
end do
end function commatrix

function kron(A,B)
!Kronecker product
double precision A(:,:),B(:,:)&
&,kron(1:size(A,1)*size(B,1),1:size(A,2)*size(B,2))
integer i,j, fila,filb,cola,colb
fila=size(A,1)
filb=size(B,1)
cola=size(A,2)
colb=size(B,2)
forall(i=1:fila,j=1:cola)
		kron(1+(i-1)*filb:i*filb,1+(j-1)*colb:j*colb)=A(i,j)*B
end forall
end function kron
function vec(A)
!vec operator
double precision A(:,:),vec(1:size(A,1)*size(A,2),1:1)
integer i,fil,col
fil=size(A,1)
col=size(A,2)
forall(i=1:col)
	vec(1+(i-1)*fil:i*fil,1)=A(:,i)
end forall
end function vec

function vec2(A)
!vec operator
double precision A(:,:),vec2(1:size(A,1)*size(A,2))
integer i,fil,col
fil=size(A,1)
col=size(A,2)
forall(i=1:col)
	vec2(1+(i-1)*fil:i*fil)=A(:,i)
end forall
end function vec2

function vecd(A)
!vecd operator
double precision A(:,:),vecd(size(A,1))
integer i,fil
fil=size(A,1)
forall(i=1:fil)
	vecd(i)=A(i,i)
end forall
end function vecd
function vech(M)
double precision M(:,:),vech(size(M,1)*(size(M,1)+1)/2)
integer n,i
n=size(M,1)
forall(i=1:n)
	vech(1+n*(i-1)-((i-2)*(i-1)/2):n*i-(i*(i-1)/2))=M(i:n,i)
end forall
end function vech


function massign(m,n,val)
integer m,n,i,j
double precision massign(1:m,1:n),val
do i=1,m,1
	do j=1,n,1
		massign(i,j)=val
	end do
end do
end function massign
function vassign(m,val)
integer m,i
double precision vassign(1:m),val
forall(i=1:m)
		vassign(i)=val
end forall
end function vassign

function linspace(inic,fin,n)
integer n,i
double precision inic,fin,linspace(n),step

if (n>1) then
step=(fin-inic)/(dble(n)-1.0d0)
else
step=0
end if

do i=1,n,1
	linspace(i)=inic+step*dble(i-1.0d0)
end do
end function linspace

function trace(Q)
double precision trace,Q(:,:)
integer n,i
n=size(Q,1)
trace=0.0d0
do i=1,n
	trace=trace+Q(i,i)
end do
end function trace

function maxmax(p)
double precision maxmax,p(:,:)
integer i,j,fil,col
fil=size(p,1)
col=size(p,2)
maxmax=p(1,1)
do i=1,fil,1
	do j=1,col,1
		if (p(i,j).gt.maxmax) then
			maxmax=p(i,j)
		end if
	end do
end do
end function maxmax
function mhadamard(m1,m2)
double precision m1(:,:),m2(:,:),mhadamard(1:size(m1,1),1:size(m1,2))
integer fil,col,i,j
fil=size(m1,1)
col=size(m1,2)
do i=1,fil,1
	do j=1,col,1
		mhadamard(i,j)=m1(i,j)*m2(i,j)
	end do
end do
end function mhadamard
!!!
function duplicmat(n)
use linear_operators
integer i,j,n
double precision Tij(n,n),uij(n*(n+1)/2,1),duplicmat(n*n,n*(n+1)/2)

forall(i=1:n,j=1:n)
	Tij(i,j)=0.0d0
end forall
forall(i=1:(n*(n+1)/2))
	uij(i,1)=0.0d0
end forall
forall(i=1:n*n,j=1:(n*(n+1)/2))
	duplicmat(i,j)=0.0d0
end forall

do j=1,n,1
do i=j,n,1
		uij((j-1)*n+i-(j*(j-1)/2),1)=1.0d0
		if (i==j) then
			Tij(i,j)=1.0d0
		else
			Tij(i,j)=1.0d0
			Tij(j,i)=1.0d0
		end if

		duplicmat=duplicmat+(vec(Tij).xt.uij)

		uij((j-1)*n+i-(j*(j-1)/2),1)=0.0d0
		if (i==j) then
			Tij(i,j)=0.0d0
		else
			Tij(i,j)=0.0d0
			Tij(j,i)=0.0d0
		end if
end do
end do

end function duplicmat

function elimmat(n)
use linear_operators
integer i,j,n
double precision Eij(n,n),uij(n*(n+1)/2,1),elimmat(n*(n+1)/2,n*n)

forall(i=1:n,j=1:n)
	Eij(i,j)=0.0d0
end forall
forall(i=1:(n*(n+1)/2))
	uij(i,1)=0.0d0
end forall
forall(i=1:(n*(n+1)/2),j=1:n*n)
	elimmat(i,j)=0.0d0
end forall

do j=1,n,1
do i=j,n,1
		uij((j-1)*n+i-(j*(j-1)/2),1)=1.0d0
		Eij(i,j)=1.0d0

		elimmat=elimmat+(uij.xt.vec(Eij))

		uij((j-1)*n+i-(j*(j-1)/2),1)=0.0d0
		Eij(i,j)=0.0d0
end do
end do

end function elimmat

function vectomat(v,m,n)
integer m,n,i,j
double precision v(m*n),vectomat(m,n)
forall(i=1:m,j=1:n)
	vectomat(i,j)=v(m*(j-1)+i)
end forall
end function vectomat

function extractis(mat,ind0,rdim)
!Extract columns and rows in ind
integer rdim,i,dm
integer ind0(rdim),ind(rdim)
double precision mat(:,:)
double precision extractis(size(mat,1)-rdim,size(mat,2)-rdim),res(size(mat,1),size(mat,2))

ind=ind0
ind=-ind
call svign(rdim,ind,ind)
ind=-ind

dm=size(mat,1)

res=mat

do i=1,rdim
	res(1:dm-i,1:dm-i)=extracti(res(1:dm-i+1,1:dm-i+1),ind(i))
end do
extractis=res(1:dm-rdim,1:dm-rdim)
end function extractis

function extracti(mat,ind)
!Extract column and row ind
integer ind,dm
double precision mat(:,:)
double precision extracti(size(mat,1)-1,size(mat,1)-1),res1(size(mat,1)-1,size(mat,1))

dm=size(mat,1)
if (ind==1) then
	extracti=mat(2:dm,2:dm)
elseif(ind==dm) then
	extracti=mat(1:dm-1,1:dm-1)
else
	res1(1:ind-1,:)=mat(1:ind-1,:)
	res1(ind:dm-1,:)=mat(ind+1:dm,:)
	extracti(:,1:ind-1)=res1(:,1:ind-1)
	extracti(:,ind:dm-1)=res1(:,ind+1:dm)
end if
end function extracti

function rextractis(mat,ind0,rdim)
!Extract rows in ind
integer rdim,i,dm
integer ind0(rdim),ind(rdim)
double precision mat(:,:)
double precision rextractis(size(mat,1)-rdim,size(mat,2)),res(size(mat,1),size(mat,2))

ind=ind0
ind=-ind
call svign(rdim,ind,ind)
ind=-ind


dm=size(mat,1)

res=mat

do i=1,rdim,1
	res(1:dm-i,:)=rextracti(res(1:dm-i+1,:),ind(i))
end do
rextractis=res(1:dm-rdim,:)
end function rextractis

function rextracti(mat,ind)
!Extract row ind
integer ind,dm
double precision mat(:,:)
double precision rextracti(size(mat,1)-1,size(mat,2))

dm=size(mat,1)
if (ind==1) then
	rextracti=mat(2:dm,:)
elseif(ind==dm) then
	rextracti=mat(1:dm-1,:)
else
	rextracti(1:ind-1,:)=mat(1:ind-1,:)
	rextracti(ind:dm-1,:)=mat(ind+1:dm,:)
end if
end function rextracti
!!!!!!!!!!!!!!!!!!!!!!!!!
function cholesky(x,dim)
use linear_operators
integer dim,irank
double precision x(dim,dim),cholesky(dim,dim)&
&,tol,dmach

tol = 100.0*dmach(4)
call dchfac(dim,x,dim,tol,irank,cholesky,dim)
cholesky=(.t.cholesky)

end function cholesky

function cov2(y,x)
double precision cov2,y(:),x(:),m1,m2
cov2=mean1(x*y)-mean1(x)*mean1(y)
end function cov2

function mean1(x)
double precision mean1,x(:)
mean1=sum(x)/size(x)
end function mean1

function mean(x)
double precision x(:,:)
double precision mean(size(x,2))
mean=sum(x,1)/size(x,1)
end function mean

function cov(x)
use linear_operators
double precision x(:,:)
double precision cov(size(x,2),size(x,2)),res(size(x,2),size(x,2)),m(size(x,2),1)
integer n,T,i,j
n=size(x,2)
T=size(x,1)
res=(x(1:1,:).tx.x(1:1,:))
do i=2,T,1
	res=res+(x(i:i,:).tx.x(i:i,:))
end do
m(:,1)=mean(x)
cov=(res/T)-(m.xt.m)
end function cov

function polar(th)
double precision, intent(in):: th(:)
double precision polar(size(th)+1),prod
integer i,dim
dim=size(th)
polar(1)=dcos(th(1))
prod=dsin(th(1))
if (dim==1) then
	polar(1)=prod
else
	do i=2,dim,1
		polar(i)=prod*dcos(th(i))
		prod=prod*dsin(th(i))
	end do
	polar(dim+1)=prod
end if
end function polar

function regress(y,x)
use linear_operators
double precision, intent(in):: y(:),x(:,:)
double precision:: regress(size(x,2)),xx(size(x,2),size(x,2)),xres(size(x,2),1),vy(size(y),1)

xx=x.tx.x
vy(:,1)=y
xres=(xx.ix.(x.tx.vy))
regress=xres(:,1)
end function regress

end module matrixfuns